#include "lis2dh12.h"
#include "x_nucleo_iks01a1.h"
#include <math.h>
#include "app.h"

static uint8_t tempReg[LIS2DH12_SAMPLE_NUMBER * 6] = {0};

LIS2DH12_StatusTypeDef LIS2DH12_Reg_Read(uint8_t reg, uint8_t* data)
{
 
    if(LIS2DH12_IO_Read(data, LIS2DH12_ADDRESS, reg, 1) != LIS2DH12_OK)
        return LIS2DH12_ERROR;
    
    return LIS2DH12_OK;
}

/*register set*/
static LIS2DH12_StatusTypeDef LIS2DH12_Reg_Set(uint8_t reg, uint8_t data, uint8_t bit_mask)
{
    uint8_t tmp1 = 0x00;
 
    if(LIS2DH12_IO_Read(&tmp1, LIS2DH12_ADDRESS, reg, 1) != LIS2DH12_OK)
        return LIS2DH12_ERROR;

    tmp1 &= ~(bit_mask);
    tmp1 |= data;
    if(LIS2DH12_IO_Write(&tmp1, LIS2DH12_ADDRESS, reg, 1) != LIS2DH12_OK)
        return LIS2DH12_ERROR;

    return LIS2DH12_OK;
}

/*EXIT IO Config*/
LIS2DH12_StatusTypeDef LIS2DH12_INT_IO_ITConfig(void)
{
    if(LIS2DH12_Sensor_IO_ITConfig() != LIS2DH12_OK)
        return LIS2DH12_ERROR;
    
    return LIS2DH12_OK;
}

/*check who am i*/
LIS2DH12_StatusTypeDef LIS2DH12_Read_Id(uint8_t* data)
{
    if(LIS2DH12_IO_Read(data, LIS2DH12_ADDRESS, LIS2DH12_REG_WHO_AM_I, 1) != LIS2DH12_OK)
        return LIS2DH12_ERROR;
       
    return LIS2DH12_OK;
}

/*io init*/
LIS2DH12_StatusTypeDef LIS2DH12_Init(void)
{
    if(LIS2DH12_IO_Init() != LIS2DH12_OK)
        return LIS2DH12_ERROR;  
    return LIS2DH12_OK;
}

/*read fifo number*/
LIS2DH12_StatusTypeDef LIS2DH12_Read_Fifo_Byte_Number(uint8_t* data)
{
    if(LIS2DH12_IO_Read(data, LIS2DH12_ADDRESS, LIS2DH12_REG_FIFO_SRC_REG, 1) != LIS2DH12_OK)
        return LIS2DH12_ERROR;
   
    return LIS2DH12_OK;
}

/*read fifo number*/
LIS2DH12_StatusTypeDef LIS2DH12_Read_INT1_SRC(uint8_t* data)
{
    if(LIS2DH12_IO_Read(data, LIS2DH12_ADDRESS, LIS2DH12_REG_INT1_SRC_REG, 1) != LIS2DH12_OK)
        return LIS2DH12_ERROR;
   
    return LIS2DH12_OK;
}

/*fifo mode set*/
LIS2DH12_StatusTypeDef LIS2DH12_FIFO_Mode_Set(uint8_t data)
{
    return LIS2DH12_Reg_Set(LIS2DH12_REG_FIFO_CTRL, data, LIS2DH12_FIFO_MODE_MASK);
}

/*reference set*/
LIS2DH12_StatusTypeDef LIS2DH12_Reference_Set(uint8_t data)
{
    return LIS2DH12_Reg_Set(LIS2DH12_REG_REFERENCE_REG, data, 0xff);
}

/* clear fifo*/
LIS2DH12_StatusTypeDef LIS2DH12_FIFO_Buffer_Reset(void)
{
    if(LIS2DH12_FIFO_Mode_Set(LIS2DH12_FIFO_MODE_BYPASS) != LIS2DH12_OK)
         return LIS2DH12_ERROR;
    
    if(LIS2DH12_FIFO_Mode_Set(LIS2DH12_FIFO_MODE_STREAM) != LIS2DH12_OK)
         return LIS2DH12_ERROR;
    
    return LIS2DH12_OK;
}

/*fifo fth value set*/
LIS2DH12_StatusTypeDef LIS2DH12_TR_Select(uint8_t data)
{
    return LIS2DH12_Reg_Set(LIS2DH12_REG_FIFO_CTRL, data, LIS2DH12_FIFO_TR_INT_MASK);
}

/*fifo fth value set*/
LIS2DH12_StatusTypeDef LIS2DH12_FIFO_FTH_Value_Set(uint16_t data)
{
    return LIS2DH12_Reg_Set(LIS2DH12_REG_FIFO_CTRL, data, LIS2DH12_FIFO_FTH_VALUE_MASK);
}

/*fifo fth int set*/
LIS2DH12_StatusTypeDef LIS2DH12_FIFO_WTM_INT1_Enable(uint8_t data)
{
    return LIS2DH12_Reg_Set(LIS2DH12_REG_CTRL_REG3, data, LIS2DH12_WTM_INT1_ENABLE_MASK);
}

/*over run set*/
LIS2DH12_StatusTypeDef LIS2DH12_FIFO_OVERRUN_INT1_Enable(uint8_t data)
{
    return LIS2DH12_Reg_Set(LIS2DH12_REG_CTRL_REG3, data, LIS2DH12_OVERRUN_INT1_ENABLE);
}

/*odr set*/
LIS2DH12_StatusTypeDef LIS2DH12_Data_Rate_Set(uint8_t data)
{
    return LIS2DH12_Reg_Set(LIS2DH12_REG_CTRL_REG1, data, LIS2DH12_ODR_MASK);
}

/*BDU set*/
LIS2DH12_StatusTypeDef LIS2DH12_BDU_Set(uint8_t data)
{
    return LIS2DH12_Reg_Set(LIS2DH12_REG_CTRL_REG4, data, LIS2DH12_BDU_SET_MASK);
}

/*Full-scale set*/
LIS2DH12_StatusTypeDef LIS2DH12_FS_Set(uint8_t data)
{
    return LIS2DH12_Reg_Set(LIS2DH12_REG_CTRL_REG4, data, LIS2DH12_FS_SET_MASK);
} 

/*power mode set*/
LIS2DH12_StatusTypeDef LIS2DH12_Power_Mode_Set(uint8_t data)
{
    uint8_t lpen_bit = 0, hr_bit = 0;
 
    lpen_bit = (data & 0x02) << 2;
    hr_bit = (data & 0x01) << 3;
    
    if(LIS2DH12_Reg_Set(LIS2DH12_REG_CTRL_REG1, lpen_bit, LIS2DH12_POWER_MODE_MASK) != LIS2DH12_OK)
        return LIS2DH12_ERROR;

    return LIS2DH12_Reg_Set(LIS2DH12_REG_CTRL_REG4, hr_bit, LIS2DH12_POWER_MODE_MASK); 
}

/*fifo enable*/
LIS2DH12_StatusTypeDef LIS2DH12_FIFO_Enable(uint8_t data)
{
    return LIS2DH12_Reg_Set(LIS2DH12_REG_CTRL_REG5, data, LIS2DH12_FIFO_ENABLE_MASK);
} 

/*latch int on int1_src*/
LIS2DH12_StatusTypeDef LIS2DH12_Latch_Int_On_Int1_Src(uint8_t data)
{
    return LIS2DH12_Reg_Set(LIS2DH12_REG_CTRL_REG5, data, LIS2DH12_LATCH_INT1_ENABLE_MASK);
}

/*x, y, z axis enable*/
LIS2DH12_StatusTypeDef LIS2DH12_Axis_Enable(uint8_t data)
{
    return LIS2DH12_Reg_Set(LIS2DH12_REG_CTRL_REG1, data, LIS2DH12_AXIS_ENABLE_MASK);
}

/*selftest mode set*/
LIS2DH12_StatusTypeDef LIS2DH12_Selftest_Mode_Set(uint8_t data)
{
    return LIS2DH12_Reg_Set(LIS2DH12_REG_CTRL_REG4, data, LIS2DH12_SELFTEST_MODE_MASK);
}

/*get x, y, z axis raw data*/

LIS2DH12_StatusTypeDef LIS2DH12_X_Y_Z_Get_Axes_Raw(void)
{

    if(LIS2DH12_IO_Read(&tempReg[0], LIS2DH12_ADDRESS, LIS2DH12_REG_OUT_FIFO, LIS2DH12_SAMPLE_NUMBER*6) != LIS2DH12_OK)
        return LIS2DH12_ERROR;    

    return LIS2DH12_OK;
}

void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *I2CxHandle)
{
    app_i2c_dma_rx_cmp_callback(tempReg);
}

/*get sensitivity*/
LIS2DH12_StatusTypeDef LIS2DH12_Get_Sensitivity(float* sensitivity)
{
    uint8_t tmp1 = 0, full_scale_factor;
    /*read full scall*/
    if(LIS2DH12_IO_Read(&tmp1, LIS2DH12_ADDRESS, LIS2DH12_REG_CTRL_REG4, 1) != LIS2DH12_OK)
        return LIS2DH12_ERROR;
    
    tmp1 &= LIS2DH12_FS_SET_MASK;
    switch(tmp1){
        case LIS2DH12_FS_2G:
            full_scale_factor = LIS2DH12_2G_SENSITIVITY;
        break;
        case LIS2DH12_FS_4G:
            full_scale_factor = LIS2DH12_4G_SENSITIVITY;
        break;
        case LIS2DH12_FS_8G:
            full_scale_factor = LIS2DH12_8G_SENSITIVITY;
        break;
        case LIS2DH12_FS_16G:
            full_scale_factor = LIS2DH12_16G_SENSITIVITY;
        break;
        default:

        break;
    }
   
    *sensitivity = (float)full_scale_factor / 32768;
    
    return LIS2DH12_OK;
} 

/*get x, y, z axis raw data*/
LIS2DH12_StatusTypeDef LIS2DH12_X_Y_Z_Get_Axes_Data(float *pData)
{
    /*Here we have to add the check if the parameters are valid*/
    static uint8_t i = 0;
    static float sensitivity = 0.0;
    lis2dh12_axis_type_t pDataRaw[3] = {0, 0, 0};
    
    if(LIS2DH12_X_Y_Z_Get_Axes_Raw() != LIS2DH12_OK)
        return LIS2DH12_ERROR;
     
    if(i == 0){
        if(LIS2DH12_Get_Sensitivity( &sensitivity ) != LIS2DH12_OK)
            return LIS2DH12_ERROR;
        i++;
    }else{
        i = 1;
    }
    

    pData[0] = (float)(pDataRaw->axis_x * sensitivity);
    pData[1] = (float)(pDataRaw->axis_y * sensitivity);
    pData[2] = (float)(pDataRaw->axis_z * sensitivity);

    return LIS2DH12_OK;
}


